import json
import os
import time
import sys
from datetime import datetime, timedelta
from pyproj import Proj


# blueprintsWalkers = world.get_blueprint_library().filter('walker.pedestrian.*')
rule = Proj("+proj=tmerc +lat_0=36.63498592769697 +lon_0=117.5620618908321 +ellps=WGS84")

vtype_map = {'0': 'vehicle.lincoln.mkz_2020', '1': 'vehicle.lincoln.mkz_2020', '2': 'vehicle.ford.ambulance',
             '3': 'vehicle.mercedes.sprinter',
             '4': 'walker.pedestrian.0013', '5': 'vehicle.gazelle.omafiets', '6': 'vehicle.vespa.zx125',
             '10': 'vehicle.carlamotors.carlacola', '11': 'vehicle.ford.ambulance', '13': 'vehicle.carlamotors.firetruck',
             '150': 'vehicle.vespa.zx125'}

def indent(elem, level=0):
    i = "\n" + level * "  "
    if len(elem):
        if not elem.text or not elem.text.strip():
            elem.text = i + "  "
        if not elem.tail or not elem.tail.strip():
            elem.tail = i
        for elem in elem:
            indent(elem, level + 1)
        if not elem.tail or not elem.tail.strip():
            elem.tail = i
    else:
        if level and (not elem.tail or not elem.tail.strip()):
            elem.tail = i


def str2date(dstr, format='%Y.%m.%d-%H.%M.%S.%f'):
    return datetime.strptime(dstr, format)


def get_timediff(st, et):
    return (str2date(et) - str2date(st)).total_seconds()


def get_time_stamp(ct):
    """

    :param ct: float时间
    :return: 带毫秒的格式化时间戳
    """
    # ct = time.time()
    # print(ct)
    local_time = time.localtime(ct)
    # data_head = time.strftime("%Y-%m-%d %H:%M:%S", local_time)
    data_head = time.strftime("%Y.%m.%d-%H.%M.%S.%f", local_time)
    data_secs = (ct - int(ct)) * 1000
    time_stamp = "%s.%03d" % (data_head, data_secs)
    # print(time_stamp, type(time_stamp))
    # stamp = ("".join(time_stamp.split()[0].split("-"))+"".join(time_stamp.split()[1].split(":"))).replace('.', '')
    # print(stamp)
    return time_stamp


def get_e1(input=r'/home/gj/Download/result51.json', t=None):
    final_dic = {}
    outdic = {}
    carbox = {}
    start_time = ''

    k = 0
    with open(input) as f:
        while True:
            line = f.readline()
            if line:
                if "e1FrameParticipant" in line:
                    framedata = eval(line)
                    timeStamp = framedata['timestamp'] / 1000.0
                    # timeStamp = float(framedata['frameID'])/10.0 # 跟踪数据中的时间戳相同，这里不采用该时间戳
                    # new_timeStamp = get_time_stamp(int(timeStamp) / 1000)
                    new_timeStamp = timeStamp
      
                    if True:
                        if k == 0:
                            start_time = new_timeStamp
                            k += 1
                        participantNum = framedata['participantNum']
                        participants = framedata['e1FrameParticipant']
                        
                        outdic[new_timeStamp] = {}
                        for p in participants:
                            id = p['id']
                            outdic[new_timeStamp][id] = {}
                            longitude = p['longitude']
                            latitude = p['latitude']
                            altitude = 0.0
                            # color = p['color']
                            courseAngle = p['courseAngle']
                            # length = p['length']
                            # height = p['height']
                            # width = p['width']
                            cartype = p['vehicleType']
                            # if cartype not in carbox.keys():
                            #     carbox[cartype] = [length, width, height]
                            # outdic[new_timeStamp][id]['longitude'] = longitude
                            # outdic[new_timeStamp][id]['latitude'] = latitude
                            # outdic[new_timeStamp][id]['altitude'] = altitude
                            # outdic[new_timeStamp][id]['color'] = color
                            # outdic[new_timeStamp][id]['courseAngle'] = courseAngle
                            # outdic[new_timeStamp][id]['length'] = length
                            # outdic[new_timeStamp][id]['height'] = height
                            # outdic[new_timeStamp][id]['width'] = width
                            outdic[new_timeStamp][id]['type'] = vtype_map[str(cartype)]
                            x, y = rule(longitude, latitude)
                            # transform = "Transform(Location(x={0}, y={1}, z=38.0),Rotation(pitch=0.000000," \
                            #             " yaw={2}, roll=0.000000))".format(str(x), str(0-y), str(courseAngle - 90))
                            outdic[new_timeStamp][id]['Location'] = [x, 0-y, 0]
                            outdic[new_timeStamp][id]['Rotation'] = [0, courseAngle-90, 0.0]

            else:
                break

    #保存
    carid_file = json.dumps(outdic, ensure_ascii=False)
    out_file = open(r'/home/gj/Carla_0.9.13/PythonAPI/work/tessng_track2.json', 'w', encoding='utf8')
    out_file.write(carid_file)
    return start_time


if __name__ == '__main__':
    start_time = get_e1()
    print(start_time)

